/**
 * @file ndt_localizer.hpp
 * @brief NDT (Normal Distributions Transform) 定位器的头文件
 * 
 * 该文件定义了基于NDT算法的点云定位器，用于在已知地图中定位机器人位置。
 * 通过订阅激光雷达点云数据，与预先加载的地图进行匹配，计算出机器人的精确位置。
 */

#ifndef AUTO_NAVIGATION_NDT_LOCALIZER_HPP
#define AUTO_NAVIGATION_NDT_LOCALIZER_HPP

#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_eigen/tf2_eigen.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Float32.h>

#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>

#include <string>
#include <memory>
#include <vector>
#include <numeric>
#include <Eigen/Dense>

namespace auto_navigation {

/**
 * @class NDTLocalizer
 * @brief 基于NDT算法的点云定位器类
 * 
 * 该类实现了基于NDT (Normal Distributions Transform) 算法的点云定位功能。
 * 通过将当前激光雷达点云与预先构建的地图点云进行匹配，计算出机器人的精确位置。
 * 
 * ROS话题:
 * 订阅:
 *   - /points_raw (sensor_msgs/PointCloud2): 原始激光雷达点云数据
 *   - /initialpose (geometry_msgs/PoseWithCovarianceStamped): 初始位姿估计
 * 
 * 发布:
 *   - /ndt/current_pose (geometry_msgs/PoseStamped): 当前估计的机器人位姿
 *   - /ndt/current_scan (sensor_msgs/PointCloud2): 当前处理后的激光雷达点云
 *   - /ndt/map_cloud (sensor_msgs/PointCloud2): 加载的地图点云
 *   - /ndt/score (std_msgs/Float32): NDT匹配得分
 *   - /ndt/pose (nav_msgs/Odometry): NDT位姿信息
 * 
 * TF转换:
 *   - map -> base_link: 发布地图到机器人基座的转换
 */
class NDTLocalizer {
public:
    /**
     * @brief 构造函数
     * @param nh ROS节点句柄
     * @param private_nh ROS私有节点句柄
     */
    NDTLocalizer(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
    
    /**
     * @brief 析构函数
     */
    ~NDTLocalizer();

private:
    // ROS相关
    ros::NodeHandle& nh_;
    ros::NodeHandle& private_nh_;
    
    // 定时器
    ros::Timer map_publish_timer_;  // 定时发布地图点云
    
    // 订阅器
    ros::Subscriber points_sub_;    // 激光雷达点云订阅
    ros::Subscriber initial_pose_sub_; // 初始位姿订阅
    
    // 发布器
    ros::Publisher current_pose_pub_; // 当前位姿发布
    ros::Publisher current_scan_pub_; // 当前点云发布
    ros::Publisher map_cloud_pub_;    // 地图点云发布
    ros::Publisher ndt_score_pub_;     // NDT分数发布
    ros::Publisher ndt_pose_pub_;     // NDT位姿发布
    ros::Publisher ndt_stat_pub_;     // NDT状态发布
    
    // TF相关
    tf2_ros::TransformBroadcaster tf_broadcaster_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    // PCL NDT对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_;
    
    // 点云和地图数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;      // 全局地图点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud_;  // 当前帧点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_scan_;   // 当前扫描点云
    
    // 位姿相关
    Eigen::Matrix4f current_pose_;    // 当前位姿
    Eigen::Matrix4f previous_pose_;   // 上一帧位姿
    Eigen::Matrix4f initial_pose_;    // 初始位姿
    bool is_first_frame_;            // 是否是第一帧
    bool has_initial_pose_;          // 是否有初始位姿
    
    // NDT参数
    double voxel_leaf_size_;     // 体素滤波大小
    double ndt_resolution_;      // NDT网格分辨率
    double ndt_step_size_;       // NDT优化步长
    double ndt_epsilon_;         // NDT收敛阈值
    double transformation_epsilon_; // 变换收敛阈值
    int max_iterations_;         // 最大迭代次数
    double step_size_;           // 步长
    
    // 坐标系参数
    std::string map_frame_id_;     // 地图坐标系
    std::string base_frame_id_;    // 机器人基座坐标系
    std::string lidar_frame_id_;   // 激光雷达坐标系
    
    // 地图文件路径
    std::string map_file_path_;    // 地图文件路径
    
    // 定位评估相关
    std::vector<double> recent_scores_;   // 最近的NDT分数
    int score_window_size_;              // 评分窗口大小
    double score_threshold_;             // 评分阈值
    
    // 回调函数
    void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& input_cloud);
    void pointsCallback(const sensor_msgs::PointCloud2::ConstPtr& input_cloud_msg);
    void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& input_pose);
    
    // 功能函数
    void initializeParams();
    bool loadMap();
    bool loadMap(const std::string& map_file);
    void publishPose();
    void publishCurrentScan();
    bool getInitialGuess(Eigen::Matrix4f& initial_guess);
    
    // 辅助函数
    void downsamplePointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr& output_cloud,
                            double leaf_size);
    
    // 定位评估函数
    double evaluateLocalizationQuality();
    bool isLocalizationStable();

    // 新增方法
    void publishMapCloud();
    void publishNDTScore(double score);
};

} // namespace auto_navigation

#endif // AUTO_NAVIGATION_NDT_LOCALIZER_HPP 